#include <iostream>
#include <string>
using namespace std;

#include <opencv4/opencv2/core/core.hpp>
#include <opencv4/opencv2/highgui/highgui.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

using PointT = pcl::PointXYZRGBA;
using PointCloud = pcl::PointCloud<PointT>;

const double camera_factor{1000}, camera_cx{325.5}, camera_cy{253.5}, camera_fx{518.0}, camera_fy{519.0};

int main(int argc, char **argv)
{
    cv::Mat rgb, depth;
    rgb = cv::imread("../data/rgb.png");
    depth = cv::imread("../data/depth.png", cv::IMREAD_UNCHANGED);
    PointCloud::Ptr cloud(new PointCloud);
    for(auto m = 0; m < depth.rows; m++)
    {
        for(auto n = 0; n < depth.cols; n++)
        {
            ushort d = depth.ptr<ushort>(m)[n];
            if(d == 0)
                continue;
            PointT p;
            p.z = double(d) / camera_factor;
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;
            p.b = rgb.ptr<uchar>(m)[n * 3];
            p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
            p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
            cloud->points.push_back(p);
        }
    }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size: " << cloud->points.size() << endl;
    pcl::io::savePCDFile("../data/cloud.pcd", *cloud);
    cloud->points.clear();
    cout << "Point cloud saved." << endl;
    return 0;
}
